package com.jay.example.IndoorPositioningSystem;
/****
 * https://gitee.com/unw1900/Indoor_Positioning_System
 */

import android.util.Log;

import static java.lang.Math.PI;

class Kalman {

    private double startValue;  //k-1时刻的滤波值，即是k-1时刻的值
    private double kalmanGain;   //   Kalamn增益
    private double A;   // x(n)=A*x(n-1)+u(n),u(n)~N(0,Q)
    private double H;   // z(n)=H*x(n)+w(n),w(n)~N(0,R)
    private double Q;   //预测过程噪声偏差的方差
    private double R;   //测量噪声偏差，(系统搭建好以后，通过测量统计实验获得)
    private double P;   //估计误差协方差;
    private int pointer = 0;
    private int gaussianWindowSize = 8;
    private int[] accbuf = new int[gaussianWindowSize];

//#endif // KALMAN_H

    /**
     * 初始化
     * @param Q [过程噪声，Q增大，动态响应变快，收敛稳定性变坏] or 代表预测模型本身带来的噪声
     * @param R [测量噪声，R增大，动态响应变慢，收敛稳定性变好] or 代表观察噪声
     */
    Kalman(double Q, double R)//预测误差的方差,噪声误差的方差
    {//初始化
        A = 1;
        H = 1;
        P = 10;//下一时刻的协方差，初始化随意
        this.Q = Q;
        this.R = R;
        startValue=-90;
    }

    double KalmanFilter( double value)//传入测量值
    {
        //预测下一时刻的值，控制量Ut=0,故省去
        double predictValue = A* startValue;
        //求预测下一时刻的协方差
        P = A*A*P + Q;  //计算先验均方差 p(n|n-1)=A^2*p(n-1|n-1)+q
        //计算kalman增益
        kalmanGain = P*H / (P*H*H + R);  //Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
        //修正结果，即计算滤波值
        startValue = predictValue + kalmanGain*(value - predictValue);  //利用残余的信息改善对x(t)的估计，给出后验估计，这个值也就是输出  X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))
        //更新后验估计
        P = (1 - kalmanGain*H)*P;//计算后验均方差  P[n|n]=(1-K[n]*H)*P[n|n-1]

        return  startValue;
    }



    /*******************
     * 滑动滤波：将输入序列右移 1 位，先进先出，对序列求平均值，序列长度为：WINDOWS
     * @param data 最新数据
     * @return 窗口内数据平均后输出
     */
    int movingGaussianFilter(int data) {
        int outData;
        if(pointer < gaussianWindowSize) {
            accbuf[pointer++] = data;
        } else {
            if (accbuf.length - 1 >= 0)
                System.arraycopy(accbuf, 1, accbuf, 0, accbuf.length - 1);//数组右移1位
            accbuf[accbuf.length-1] = data;//最后一位补新数据
        }
        outData = (int) gaussianFilter(3,0.6, accbuf);
        return outData;
    }

    double gaussianFilter(int mu, double sigma, int[] data){// mu取3？
        int r = (int) (Math.pow(mu,2) - 1); // r = 8
        double[] fliterdata = new double[data.length];
        double[] gaussTemp = new double[r];//模板（窗口）长度为8，[0:7],中间值为2
        double sumOfGauss = 0;
        double sumOfFliter = 0;

        for (int x=1; x<=r-1; x++){//获取高斯模板,长度为5，即[0:4]
            gaussTemp[x-1] = Math.exp(-Math.pow(x-mu,2)/(2 * Math.pow(sigma,2)))/(sigma* Math.sqrt(2*PI));//一维高斯函数(二项分布)
        }
//        Log.d("myScanCallback", "\n------------" + Arrays.toString(gaussTemp) +"----------");
        for (int y=1; y<r-1; y++){//数据
              fliterdata[y-1] = data[y-1]*gaussTemp[y-1];
        }
        for (int z=0; z<=r-1; z++){ //获取总和
            sumOfGauss += gaussTemp[z];
            sumOfFliter += fliterdata[z];
        }
        Log.d("myScanCallback", "\n------------" + sumOfFliter/sumOfGauss +"----------");
        return sumOfFliter/sumOfGauss;
    }

}
